#include <rtthread.h>
#include "drv_canthread.h"
#include <rtdevice.h>
#include "ThreadCtrl.h"
#include "drv_motor.h"

static struct rt_semaphore can1_rx_sem; // 用于接收消息的信号量

rt_device_t can1_dev; // CAN 设备句柄

/**
 * @brief  can1接收回调
 * @retval RT_EOK
 */
static rt_err_t Can1_Rx_Call(rt_device_t dev, rt_size_t size)
{
    // CAN 接收到数据后产生中断，调用此回调函数，然后发送接收信号量
    rt_sem_release(&can1_rx_sem);
    return RT_EOK;
}

/**
 * @brief：电机转动量计算（跨圈处理）
 * @param float Angle1: 要转到的角度对应的数值
 * @param float Angle2: 转动起点数值
 * @param float Round_Len: 电机整圈对应的数值
 * @return float: 输出从Angle2到Angle1需要的最短路径的距离长度, 范围 负半圈~正半圈，正好半圈时取正半圈
 * @author：ych
 */
float Motor_Get_DeltaAngle(float Angle1, float Angle2, float Round_Len)
{
    float DAngle; // HalfRound;
    float HalfRound;
    DAngle = Angle1 - Angle2;
    HalfRound = Round_Len / 2;
    if (DAngle >= 0)
    {
        if (DAngle > HalfRound)
            DAngle -= Round_Len;
    }
    else
    {
        if (DAngle <= -HalfRound)
            DAngle += Round_Len;
    }
    return DAngle;
}

/**
 * @brief  对反馈角度进行换算为0-8191
 * @param  motor：电机数据结构体
 */
static void motor_angle_adjust(DjiMotor_t *motor)
{
    float angletemp = 0;
    rt_int32_t LEN;

    if (motor->oldangle_state == RT_ERROR)
    { // 第一次
        motor->oldangle_state = RT_EOK;
        motor->old_angle = motor->angle; // 第一次记录上次值为当前值
        motor->extra_angle = 0;
        motor->Data_Valid = 1;
        motor->loop = 0; // motor初始化前CAN就会有通信，所以初始化后的第一次CAN通信时程序才运行到这里，此时需要清空loop的数值
    }
    else
    {
        LEN = motor->Round_Len;
        angletemp = Motor_Get_DeltaAngle(motor->angle, motor->old_angle, LEN) / motor->ratio / LEN * 360.0f + motor->extra_angle; // 角度积分
        if (angletemp > 360.f)
        { // 积分大于一圈
            motor->extra_angle = angletemp - 360.f;
            motor->loop += 1;
        }
        else if (angletemp < 0)
        { // 积分小于0
            motor->extra_angle = angletemp + 360.f;
            motor->loop -= 1;
        }
        else
        {                                   // 正常
            motor->extra_angle = angletemp; // 更新积分值
        }
        motor->old_angle = motor->angle;
    }
}
// 解析接收到的电机速度、角度、电流、温度等相关数据
void motor_feedback(uint8_t *rxmsg, DjiMotor_t *motor)
{
    motor->speed = (rt_int16_t)(rxmsg[2] << 8 | rxmsg[3]);
    motor->angle = (rt_int16_t)((rxmsg[0] << 8) + rxmsg[1]); // 转子角度
    motor->current = (rt_int16_t)(rxmsg[4] << 8 | rxmsg[5]);
    motor->temperature = rxmsg[6];
    motor_angle_adjust(&motor_data.dji);
    motor->FreshTick = rt_tick_get();
}

// 对接收到的电机的反馈数据进行解析
static void can1_rec(struct rt_can_msg *msg)
{
    motor_feedback(msg->data, &motor_data.dji);
}

/**
 * @brief  can1读取线程
 */
static void Can1_Rx_Thread(void *parameter)
{
    struct rt_can_msg rxmsg = {0};

    while (1)
    {
        // hdr 值为 - 1，表示直接从 uselist 链表读取数据
        rxmsg.hdr = -1;
        // 阻塞等待接收信号量
        rt_sem_take(&can1_rx_sem, RT_WAITING_FOREVER);
        // 从 CAN 读取一帧数据
        rt_device_read(can1_dev, 0, &rxmsg, sizeof(rxmsg));
        can1_rec(&rxmsg);
    }
}
/**
 * @brief  can1初始化，can1数据处理线程和中断设定
 * @param  None
 * @retval rt_err_t
 */
rt_err_t Can_Init(void)
{
    rt_err_t res = RT_EOK;
    rt_thread_t can_thread;

    // can接收中断信号量的初始化
    res = rt_sem_init(&can1_rx_sem, "can1_sem", 0, RT_IPC_FLAG_FIFO);
    if (res != RT_EOK)
        return res;

    // 查找can设备
    can1_dev = rt_device_find(MOTOR_CAN_DEVICE_NAME);
    // 打开can设备
    res = rt_device_open(can1_dev, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX);
    if (res != RT_EOK)
        return res;
    // 设置波特率为1MHz
    res = rt_device_control(can1_dev, RT_CAN_CMD_SET_BAUD, (void *)CAN1MBaud);
    if (res != RT_EOK)
        return res;
    // 设置工作模式
    res = rt_device_control(can1_dev, RT_CAN_CMD_SET_MODE, (void *)RT_CAN_MODE_NORMAL);
    if (res != RT_EOK)
        return res;

    // 设置接收回调函数
    res = rt_device_set_rx_indicate(can1_dev, Can1_Rx_Call);
    if (res != RT_EOK)
        return res;

    // can线程初始化
    can_thread = rt_thread_create("can1_rx", Can1_Rx_Thread, RT_NULL,
                                  THREAD_STACK_CAN1RX,
                                  THREAD_PRIO_CAN1RX,
                                  THREAD_TICK_CAN1RX);
    if (can_thread != RT_NULL)
    {
        res = rt_thread_startup(can_thread);
        if (res != RT_EOK)
            return res;
    }
    else
    {
        return RT_ERROR;
    }
    return RT_EOK;
}